import time

from tb662fng_driver import MotorDriver


class StandPID:
    def __init__(self, kp, kd):
        self.kp = kp
        self.kd = kd
        self.motor = MotorDriver()
        self.last_roll = 0
        self.last_gyro_x = 0

    def update_speed(self, roll, gyro_x, target_roll=0):
        """
        角度环 （直立环）
        :param roll:
        :param gyro_x:
        :param target_roll:
        :return:
        """
        roll = 0.2 * roll + (1 - 0.2) * self.last_roll  # 低通滤波
        gyro_x = 0.9 * gyro_x + (1 - 0.9) * self.last_gyro_x

        self.last_roll = roll
        self.last_gyro_x = gyro_x
        error = roll - target_roll
        output = int(self.kp * error + self.kd * gyro_x)
        if output > 1023:
            output = 1023
        if output < -1023:
            output = -1023
        self.motor.set_left_motor_speed(output)
        time.sleep(0.01)
        self.motor.set_right_motor_speed(output)
        return output

